! Constant variable to provide frequent calls
module used_var
    implicit none

    ! Pi 
    real(8), parameter :: pi = 3.1415926 

    ! Acceleration of gravity,unit：m/s^2
    real(8), parameter :: gravity_g = 9.8 

end module used_var

! Set material parameters and net size
module input_module
    use used_var
    implicit none

        !0:静态计算,1:动态计算
        integer, parameter :: dynamic = 1

        ! 计算时长 , unit：时间步
        integer, parameter :: calcul_time = 10 

        ! Density , unit：kg/m^3
        real, parameter :: p = 1140 

        ! Diameter , unit：m
        real, parameter :: Diameter = 0.15
        
        ! Elasticity , unit：Pa
        real, parameter :: E = 8.3E9

        ! shear modulus , unit：Pa 
        real, parameter :: G = 3.2E9 

        ! sectional area ,unit：
        real, parameter :: A = pi * (Diameter**2) / 4 

        ! rotational inertia
        real, parameter :: I = pi * (Diameter**4) / 64 

        ! polar moment of inertia
        real, parameter :: Ip = 2*I 

        ! 抗扭惯性矩
        real, parameter :: It = pi * (Diameter**3) / 16 

        ! 网目长度0
        real, parameter :: L0 = 1 

        ! z方向网目节点个数
        integer, parameter :: height = 11

        ! y方向网目节点个数
        integer, parameter :: width = 11

        ! 网目形状,0为方形，1为菱形
        integer, parameter :: shape = 0

        !节点总数
        integer :: numNode

        !单元总数
        integer :: numEle

        !总自由度个数
        integer :: numDOF 

        !受约束的自由度个数
        integer :: numrestrain

        !不受约束的自由度个数
        integer :: numactive

        !建模单元计数
        integer :: elecont = 1

contains

    subroutine calculateNumNodeAndNumEle()
        implicit none

        !获得不同网目形状的节点数
        if (shape == 0) then
            numNode = width * height
        else
            numNode = height * width + (height - 1) * (width - 1)
        end if

        !获得不同网目形状的单元数
        if (shape == 0) then
            numEle = (width - 1) * height + (height - 1) * width
        else
            numEle = 4 * (height - 1) * (width - 1)
        end if

        !计算总自由度个数
        numDOF = numNode*6 

        !受约束的自由度个数
        if (shape == 0) then
            numrestrain = (height*2+width*2-4)*6 
        else
            !菱形网目仅固定顶线
            numrestrain = width*6
        end if

        !不受约束的自由度个数
        numactive = numDOF-numrestrain 

    end subroutine calculateNumNodeAndNumEle  

end module input_module

! 流体环境变量以及逐步积分参数设置
module envir_param
    use used_var
    implicit none

    !环境参数设置
    !水流速度
    real,parameter :: V = 1 

    !水流与x轴的夹角，xy方向为正
    real,parameter :: deg = 0 

    !x方向水流速度分量
    real,parameter :: Vx = V*cos(pi*deg/180)

    ! y方向水流速度分量
    real,parameter :: Vy = V*sin(pi*deg/180)

    !附加质量系数
    real,parameter :: Ca = 1

    !局部y方向水流的加速度
    real,parameter :: V_a2 = 0

    !局部z方向水流的加速度
    real,parameter :: V_a3 = 0

    !海水密度kg/m^3
    real,parameter :: pw = 1.025*1000 

    !水动力阻力系数
    real,parameter :: Cd = 1 

    !Rayleigh 结构阻尼系数
    real,parameter :: damp_param_2 = 0.1 

    !动态参数设置
    !时间步
    real,parameter :: t_gap = 0.363 

    !采用Newmark法进行逐步积分时的计算参数
    real,parameter  :: c0 = 4.0*(1.0/t_gap)**2,c1 = 2.0/t_gap,c2 = 4.0/t_gap,c3 = 1.0, &
        c4 = 1,c5 = 0,c6 = t_gap*0.5,c7 = 0.5*t_gap
    
end module envir_param

! 声明一些贯穿于整个计算程序的数组
! 用于动态分配
module array_statement
    use input_module
    implicit none

    !分配到节点的总外载荷
    real, dimension (:), allocatable :: force

    !分配到节点的重力和浮力
    real, dimension (:), allocatable :: force_static

    !节点位移
    real, dimension (:), allocatable :: node_disp

    !迭代更新后的节点位移
    real, dimension (:), allocatable :: node_disp_new

    !节点位移增量
    real, dimension (:), allocatable :: disp_gap

    !节点速度
    real, dimension (:), allocatable :: node_speed

    !迭代更新后的节点速度
    real, dimension (:), allocatable :: node_speed_new

    !节点加速度
    real, dimension (:), allocatable :: node_acc_speed

    !迭代更新后的节点加速度
    real, dimension (:), allocatable :: node_acc_speed_new

    !单元内力
    real, dimension (:), allocatable :: force_internal

    !单元整体刚度矩阵
    real, dimension (:,:), allocatable :: stiffness

    !单元整体质量矩阵
    real, dimension (:,:), allocatable :: total_M

    !单元整体阻尼矩阵
    real, dimension (:,:), allocatable :: damp_matrix

    !几何非线性整体刚度矩阵
    real, dimension (:,:), allocatable :: nonlinear_stiffness

    !节点坐标
    real, dimension (:,:), allocatable :: nodeCoord

    !单元节点分配
    integer, dimension (:,:), allocatable :: EleNode

    !受约束的节点自由度
    integer, dimension (:), allocatable :: restrainedDof

    !不受约束的节点自由度
    integer, dimension (:), allocatable :: activeDof
end module 

! 建立网衣节点和单元模型
subroutine modeling(force_static,nodeCoord,EleNode)
    !param1:静力
    !param2:节点坐标
    !param3:单元节点

    use input_module
    implicit none
    real :: force_static(numDOF)
    real :: nodeCoord(numNode,3)
    integer :: EleNode(numEle,2)

    !存储节点坐标差值
    real :: dNode(3)

    !下一个节点坐标
    real :: rowToFind(3)

    !按坐标查找节点
    integer :: matchingRows

    !记录上一个节点
    integer :: matchingRows_old

    !临时参数
    integer :: temp_i,temp_j,temp,ii,jj,kk,mm,label

    !设置输出文件
    integer :: file1,file2
    character(len=30) :: filename1 = "result/nodes.txt" 
    character(len=30) :: filename2 = "result/elements.txt"   

    !初始化静力
    force_static(:) = 0
    nodeCoord(:,:) = 0
    EleNode(:,:) = 0

    print*,"test1:number of node and element"
    print*,"numNode",numNode
    print*,"numEle",numEle

    !建立方形或者菱形网目模型
    if (shape == 1) then

        ! 输入节点和单元信息
        ! 建立边框节点
        do ii = 1, height-2
            ! z轴方向的一条边上的节点
            nodeCoord(ii,:) = [0.0, 0.0, ii*2**0.5*L0] 
            print*,"nodeCoord(ii,:)",nodeCoord(ii,:)
        end do

        do ii = 1, width
            ! 底线上的节点
            nodeCoord(ii+height-2,:) = [0.0, (ii-1)*2**0.5*L0, 0.0] 
            print*,"nodeCoord(ii+height-2,:)",nodeCoord(ii+height-2,:)
        end do

        do ii = 1, height-2
            ! z轴方向的另一条边上的节点
            nodeCoord(ii+height+width-2,:) = [0.0, (width-1)*2**0.5*L0, ii*2**0.5*L0] 
            print*,"nodeCoord(ii+height+width-2,:)",nodeCoord(ii+height+width-2,:)
        end do

        do ii = 1, width
            ! 顶线上的节点
            nodeCoord(ii+2*height+width-4,:) = [0.0, (ii-1)*2**0.5*L0, (height-1)*2**0.5*L0] 
            print*,"nodeCoord(ii+2*height+width-4,:)",nodeCoord(ii+2*height+width-4,:)
        end do

        print*,"test2"

        !建立所有内部节点
        !正45°角单元建模
        temp = 2*width+2*height-3
        do ii = 1, width+height-2
            jj = 1

            do while ((nodeCoord(ii,2)+2**0.5*L0/2*jj-((width-1)*2**0.5*L0) < 1e-4) .and. &
                (nodeCoord(ii,3)+2**0.5*L0/2*jj-((height-1)*2**0.5*L0 ) < 1e-4))
                
                !判断下一个节点是否在边框上
                if ((abs(nodeCoord(ii,2)+2**0.5*L0/2*jj-((width-1)*2**0.5*L0)) < 1e-4) .or. &
                    (abs(nodeCoord(ii,3)+2**0.5*L0/2*jj-((height-1)*2**0.5*L0)) < 1e-4)) then

                    !下一个节点坐标
                    rowToFind = [0.0, nodeCoord(ii,2)+2**0.5*L0/2*jj, nodeCoord(ii,3)+2**0.5*L0/2*jj]

                    print*,"rowToFind",rowToFind

                    !在边框节点中查找
                    do kk = width+height-1, 2*width+2*height-4
                        dNode = abs(nodeCoord(kk,:) - rowToFind)
                        label = 0
                        do mm = 1,3
                            if (dNode(mm)>1e-6) then
                                label = 1
                            end if
                        end do
                        if (label == 0) then
                            matchingRows = kk
                        end if
                    end do
                    
                    if (jj == 1) then
                        EleNode(elecont,:) = [ii, matchingRows]
                    else
                        EleNode(elecont,:) = [temp-1, matchingRows]
                    end if

                else
                    nodeCoord(temp,:) = [0.0, nodeCoord(ii,2)+2**0.5*L0/2*jj, nodeCoord(ii,3)+2**0.5*L0/2*jj]
                    if (jj == 1) then
                        EleNode(elecont,:) = [ii, temp]
                    else
                        EleNode(elecont,:) = [temp-1, temp]
                    end if
                    temp = temp + 1
                end if
                elecont = elecont + 1
                jj = jj + 1
            end do
        end do

        !输出节点信息
        do kk = 1,numNode
            print*,"nodeCoord(",kk,")=",nodeCoord(kk,:)
        end do
        !输出正45°角单元信息
        do kk = 1,elecont - 1
            print*,"EleNode(",kk,")=",EleNode(kk,:)
        end do

        !反45°角单元建模
        do ii = height-1, width+2*height-4
            jj = 1
            matchingRows_old = 0
            do while (((nodeCoord(ii,2)-2**0.5*L0/2*jj) > -1e-4) .and. &
                (((nodeCoord(ii,3)+2**0.5*L0/2*jj)-(height-1)*2**0.5*L0)< 1e-4))

                    !设置下一个节点坐标
                    rowToFind =  [0.0, nodeCoord(ii,2)-2**0.5*L0/2*jj, nodeCoord(ii,3)+2**0.5*L0/2*jj]
                    print*,"rowToFind",rowToFind
                    
                    !在边框节点中查找
                    do kk = 1, numNode 
                        dNode = abs(nodeCoord(kk,:) - rowToFind)
                        label = 0
                        do mm = 1,3
                            if (dNode(mm)>1e-6) then
                                label = 1
                            end if
                        end do
                        if (label == 0) then
                            matchingRows = kk
                            write(*,*) "found  node"
                        end if
                    end do

                    if (jj == 1) then
                        EleNode(elecont,:) = [ii, matchingRows]
                    else 
                        EleNode(elecont,:) = [matchingRows_old, matchingRows]
                    end if 

                    elecont = elecont + 1
                    matchingRows_old = matchingRows
                    jj = jj + 1
            end do
        end do 

        !输出所有单元信息
        do kk = 1,numEle
            print*,"EleNode(",kk,")=",EleNode(kk,:)
        end do
        
    else

        do temp_i = 1,width
            do temp_j = 1,height
                nodeCoord((temp_i - 1)*height + temp_j,:) = [0.0, (temp_i-1)*L0, (temp_j-1)*L0]
                !设置节点初始静力状态
                !force_static(((temp_i - 1)*height + temp_j)*6-5) = 0.0
                if (temp_i>1) then
                    EleNode(elecont,:) = [(temp_i - 2)*height+temp_j, (temp_i - 1)*height+temp_j]
                    elecont=elecont+1
                end if
            end do
            do temp_j = 1,height-1
                EleNode(elecont,:) = [(temp_i - 1)*height+temp_j, (temp_i - 1)*height+temp_j+1]
                elecont=elecont+1
            end do
        end do

        !输出所有单元信息
        do kk = 1,numEle
            print*,"EleNode(",kk,")=",EleNode(kk,:)
        end do
        
    end if

    ! 打开文件
    open(newunit = file1, file=filename1, status='replace', action='write')

    ! 将数据写入文件
    do jj = 1,numNode
        write(file1, '(ES12.3E2,1x,ES12.3E2,1x,ES12.3E2)') nodeCoord(jj,1), &
            nodeCoord(jj,2),nodeCoord(jj,3)
    end do

    ! 关闭文件
    close(file1)

    ! 打开文件
    open(newunit = file2, file=filename2, status='replace', action='write')

    ! 将数据写入文件
    do kk = 1,numEle
        write(file2, '(I5,1x,I5)') EleNode(kk,:)
    end do
    
    ! 关闭文件
    close(file2)

    return
end subroutine 

! 设置受约束的节点自由度
subroutine constraint(restrainedDof,activeDof)
    !param1:受约束的自由度
    !param2:不受约束的自由度

    use input_module
    implicit none
    integer :: restrainedDof(numrestrain)
    integer :: activeDof(numactive)
    integer :: temp_array(numDOF)

    !临时变量，用于计数
    integer :: ii
    integer :: jj
    integer :: kk
    integer :: temp1
    integer :: temp2
    integer :: temp3

    !初始化数组变量
    activeDof(:) = 0
    restrainedDof(:) = 0
    temp_array(:) = 0

    !固定四条边的节点自由度
    if (shape == 1) then
        jj = 1
        do ii = 2*height+width-3, 2*width+2*height-4
            do kk = 1,6
                restrainedDof(jj*6+kk-6) = ii*6+kk-6
            end do
            jj = jj + 1
        end do
    else
        do ii = 1,height*2+width*2-4
            if (ii <= height) then
                restrainedDof((ii-1)*6+1:(ii-1)*6+6) = [(ii-1)*6+1, (ii-1)*6+2, (ii-1)*6+3, &
                                                        (ii-1)*6+4, (ii-1)*6+5, (ii-1)*6+6]
            else if (height<ii .and. ii<=2*height) then
                temp1 = numNode-2*height+ii
                restrainedDof((ii-1)*6+1:(ii-1)*6+6) = [(temp1-1)*6+1, (temp1-1)*6+2, (temp1-1)*6+3, &
                                                        (temp1-1)*6+4, (temp1-1)*6+5, (temp1-1)*6+6]
            else if (2*height<ii .and. ii<=2*height+width-2) then
                temp2 = (ii-2*height)*height
                restrainedDof((ii-1)*6+1:(ii-1)*6+6) = [(temp2)*6+1, (temp2)*6+2, (temp2)*6+3, &
                                                        (temp2)*6+4, (temp2)*6+5, (temp2)*6+6]
            else if (2*height+width-2<ii .and. ii<=height*2+width*2-4) then
                temp3 = (ii-2*height-width+3)*height
                restrainedDof((ii-1)*6+1:(ii-1)*6+6) = [(temp3-1)*6+1, (temp3-1)*6+2, (temp3-1)*6+3, &
                                                        (temp3-1)*6+4, (temp3-1)*6+5, (temp3-1)*6+6];
            end if
        end do
    end if

    print*,"restrainedDof",restrainedDof

    !调用差集函数，得到不受约束的自由度
    do ii = 1,numDOF
        temp_array(ii) = ii
    end do
    call setdiff(temp_array,restrainedDof,activeDof,numDOF,numrestrain,numactive)

    return
end subroutine constraint

!计算单元的重力和浮力
subroutine gravity(force_static,EleNode)
    !param1:静力
    !param2:单元节点

    use input_module
    use envir_param
    use used_var
    implicit none
    real :: force_static(numDOF)
    integer :: EleNode(numEle,2)
    integer ii

    do ii = 1, numEle
        force_static(EleNode(ii,1)*6 - 3) = force_static(EleNode(ii,1)*6 - 3) - (p*gravity_g*A*L0 - pw*gravity_g*A*L0)/2
        force_static(EleNode(ii,2)*6 - 3) = force_static(EleNode(ii,2)*6 - 3) - (p*gravity_g*A*L0 - pw*gravity_g*A*L0)/2
        !输出单元重力和浮力
        write(*,*) "force_static = ",(p*gravity_g*A*L0 - pw*gravity_g*A*L0)
    end do

    return
end subroutine gravity

! 动态时历分析
subroutine time_domain_dynamic(force,nodeCoord,force_static,EleNode,activeDof)
    !param1:节点坐标
    !param2:静力
    !param3:单元节点
    !param4:不受约束的节点自由度

    use input_module
    use array_statement, ONLY: stiffness,total_M,disp_gap,node_disp,node_disp_new,node_speed,node_speed_new, &
        node_acc_speed,node_acc_speed_new,nonlinear_stiffness,force_internal
    use envir_param
    real :: nodeCoord(numNode,3)
    real :: force_static(numDOF)
    real :: Q_effective(numDOF)

    !整体阻尼矩阵
    real :: damp_matrix(numDOF,numDOF)

    !整体刚度矩阵
    real :: K_effective(numDOF,numDOF)

    !有效整体内力矩阵
    real :: effIntForce(numDOF)

    !单元节点
    integer :: EleNode(numEle,2)

    !不受约束的自由度
    integer :: activeDof(numactive)

    !迭代更新后的节点坐标
    real :: nodeCoord_new(numNode,3)

    !节点在x轴方向的坐标
    real :: xx(numNode)

    !节点在y轴方向的坐标
    real :: yy(numNode)

    !节点在z轴方向的坐标
    real :: zz(numNode)

    !有效刚度矩阵的逆矩阵
    real :: K_effective_inv(numDOF,numDOF)

    !节点总载荷
    real :: force(numDOF)

    !去除约束自由度的有效刚度矩阵的逆矩阵
    real :: K_effective_inv_temp(size(activeDof),size(activeDof))

    !临时计数变量
    integer :: ii
    integer :: jj

    !判断迭代停止
    integer :: label

    !迭代次数
    integer :: inter_count
    
    !输出文件格式
    integer :: file1,file2
    character(len=10) :: filename1 = "displacement.txt"   
    character(len=10) :: filename2 = "nodecoord.txt"              
    
    !为变量分配内存
    allocate(stiffness(numDOF,numDOF),node_disp(numDOF),disp_gap(numDOF),node_speed(numDOF),node_acc_speed(numDOF), &
        total_M(numDOF,numDOF),nonlinear_stiffness(numDOF,numDOF),force_internal(numDOF),node_disp_new(numDOF), &
        node_acc_speed_new(numDOF),node_speed_new(numDOF))

    !初始化数组变量
    node_speed(:) = 0
    node_acc_speed(:) = 0
    disp_gap(:) = 0
    node_disp(:) = 0
    Q_effective(:) = 0
    damp_matrix(:,:) = 0
    nodeCoord_new = nodeCoord
    node_disp_new(:) = 0
    node_speed_new(:) = 0
    node_acc_speed_new(:) = 0
    effIntForce(:) = 0
    K_effective_inv(:,:) = 0
    force_internal(:) = 0

    do ii = 1,calcul_time
        label = 1
        inter_count = 0
        do while (label /= 0)
            inter_count = inter_count + 1

            !每次迭代过程中，重新求解系统矩阵
            nonlinear_stiffness(:,:) = 0
            stiffness(:,:) = 0
            total_M(:,:) = 0
            force = force_static
            xx(:) = nodeCoord_new(:,1)
            yy(:) = nodeCoord_new(:,2)
            zz(:) = nodeCoord_new(:,3)

            !计算单元载荷和单元系统系统矩阵并组装
            call assembly_matrix(xx,yy,zz,EleNode,force,nonlinear_stiffness,disp_gap,stiffness,total_M,node_speed, &
                node_acc_speed,force_internal)

            !建立有效刚度矩阵并求逆
            damp_matrix = damp_param_2*stiffness
            K_effective = stiffness+c0*total_M+c1*damp_matrix + nonlinear_stiffness
            call invert_matrix(K_effective(activeDof,activeDof),K_effective_inv_temp, &
                size(activeDof))
            K_effective_inv(activeDof,activeDof) = K_effective_inv_temp

            !建立有效内载荷
            effIntForce(activeDof) = force_internal(activeDof)

            !建立有效外载荷
            Q_effective(activeDof) = force(activeDof)+matmul(total_M(activeDof,activeDof),(c0*node_disp_new(activeDof) &
                +c2*node_speed_new(activeDof) + c3*node_acc_speed_new(activeDof)))+ matmul(damp_matrix(activeDof,activeDof) &
                ,(c1*node_disp_new(activeDof)+c4*node_speed_new(activeDof) + c5*node_acc_speed_new(activeDof)))

            !基于运动平衡方程求解位移增量
            disp_gap(activeDof) = matmul(K_effective_inv(activeDof,activeDof), &
                (Q_effective(activeDof) - effIntForce(activeDof)))

            !更新上个时间步的节点位移、速度、加速度
            node_disp = node_disp_new
            node_speed = node_speed_new
            node_acc_speed = node_acc_speed_new
            node_disp_new(activeDof) = node_disp_new(activeDof) + disp_gap(activeDof)

            !更新节点坐标
            nodeCoord_new(:, 1) = nodeCoord(:, 1) + node_disp_new(1:numDOF:6)
            nodeCoord_new(:, 2) = nodeCoord(:, 2) + node_disp_new(2:numDOF:6)
            nodeCoord_new(:, 3) = nodeCoord(:, 3) + node_disp_new(3:numDOF:6) 

            !迭代更新速度
            node_acc_speed_new(activeDof)  = c0*(node_disp_new(activeDof) - node_disp(activeDof)) - c2*node_speed(activeDof) &
                - c3*node_acc_speed(activeDof)

            !迭代更新加速度
            node_speed_new(activeDof) = node_speed(activeDof) + c6*node_acc_speed(activeDof) + c7*node_acc_speed_new(activeDof)

            !判断迭代结束
            label = 0
            do jj = 1,numDOF
                if (abs(disp_gap(jj)) > 1e-7) then
                    label = 1
                end if
            end do

        end do

    end do

    ! 打开文件
    open(newunit = file1, file=filename1, status='replace', action='write')

    ! 将数据写入文件
    write(*,*) "位移"
    do jj = 1,numDOF
        write(*, '(I5,1X,ES12.3E2)') jj,node_disp_new(jj)
    end do

    ! 关闭文件
    close(file1)

    ! 打开文件
    open(newunit = file2, file=filename2, status='replace', action='write')

    ! 将数据写入文件
    do jj = 1,numNode
        write(file2, '(ES12.3E2,1x,ES12.3E2,1x,ES12.3E2)') nodeCoord_new(jj,1), &
            nodeCoord_new(jj,2),nodeCoord_new(jj,3)
    end do

    ! 关闭文件
    close(file2)

    !释放申请的变量内存
    deallocate(stiffness,node_disp,total_M,disp_gap,node_speed,node_acc_speed,nonlinear_stiffness,force_internal,node_disp_new)

    return
end subroutine 

! 静态时历分析
subroutine time_domain_static(force,nodeCoord,force_static,EleNode,activeDof)
    !param1:节点坐标
    !param2:静力
    !param3:单元节点
    !param4:不受约束的节点自由度
 
    use input_module
    use array_statement, ONLY: stiffness,total_M,disp_gap,node_disp,node_disp_new,node_speed,node_speed_new, &
        node_acc_speed,node_acc_speed_new,nonlinear_stiffness,force_internal
    use envir_param
    real :: nodeCoord(numNode,3)
    real :: force_static(numDOF)
    real :: Q_effective(numDOF)

    !整体阻尼矩阵
    real :: damp_matrix(numDOF,numDOF)

    !整体刚度矩阵
    real :: K_effective(numDOF,numDOF)

    !有效整体内力矩阵
    real :: effIntForce(numDOF)

    !单元节点
    integer :: EleNode(numEle,2)

    !不受约束的自由度
    integer :: activeDof(numactive)

    !迭代更新后的节点坐标
    real :: nodeCoord_new(numNode,3)

    !节点在x轴方向的坐标
    real :: xx(numNode)

    !节点在y轴方向的坐标
    real :: yy(numNode)

    !节点在z轴方向的坐标
    real :: zz(numNode)

    !有效刚度矩阵的逆矩阵
    real :: K_effective_inv(numDOF,numDOF)

    !节点总载荷
    real :: force(numDOF)

    !去除约束自由度的有效刚度矩阵的逆矩阵
    real :: K_effective_inv_temp(size(activeDof),size(activeDof))

    !临时计数变量
    integer :: jj

    !判断迭代停止
    integer :: label

    !迭代次数
    integer :: inter_count
    
    !输出文件格式
    integer :: file1,file2
    character(len=10) :: filename1 = "output.txt"   
    character(len=10) :: filename2 = "nodecoord.txt"          
    
    !为变量分配内存
    allocate(stiffness(numDOF,numDOF),node_disp(numDOF),disp_gap(numDOF),node_speed(numDOF),node_acc_speed(numDOF), &
        total_M(numDOF,numDOF),nonlinear_stiffness(numDOF,numDOF),force_internal(numDOF),node_disp_new(numDOF))

    !初始化数组变量
    node_disp(:) = 0
    Q_effective(:) = 0
    damp_matrix(:,:) = 0
    nodeCoord_new = nodeCoord
    node_disp_new(:) = 0
    effIntForce(:) = 0
    K_effective_inv(:,:) = 0
    force_internal(:) = 0
    node_speed(:) = 0 
    node_acc_speed(:) = 0
    disp_gap(:) = 0

    label = 1
    inter_count = 0
    do while (label /= 0)
        inter_count = inter_count + 1

        !每次迭代过程中，重新求解系统矩阵
        nonlinear_stiffness(:,:) = 0
        stiffness(:,:) = 0
        total_M(:,:) = 0
        force = force_static
        xx(:) = nodeCoord_new(:,1)
        yy(:) = nodeCoord_new(:,2)
        zz(:) = nodeCoord_new(:,3)

        print*,"test1"
        !计算单元载荷和单元系统系统矩阵并组装
        call assembly_matrix(xx,yy,zz,EleNode,force,nonlinear_stiffness,disp_gap,stiffness,total_M,node_speed, &
            node_acc_speed,force_internal)

        print*,"test2"
        !建立有效刚度矩阵并求逆
        K_effective = stiffness + nonlinear_stiffness
        call invert_matrix(K_effective(activeDof,activeDof),K_effective_inv_temp, &
            size(activeDof))
        K_effective_inv(activeDof,activeDof) = K_effective_inv_temp

        !建立有效内载荷
        effIntForce(activeDof) = force_internal(activeDof)

        !建立有效外载荷
        Q_effective(activeDof) = force(activeDof)

        !基于运动平衡方程求解位移增量
        disp_gap(activeDof) = matmul(K_effective_inv(activeDof,activeDof), &
            (Q_effective(activeDof) - effIntForce(activeDof)))
        print*,"disp_gap",disp_gap(activeDof)

        node_disp_new(activeDof) = node_disp_new(activeDof) + disp_gap(activeDof)

        !更新节点坐标
        nodeCoord_new(:, 1) = nodeCoord(:, 1) + node_disp_new(1:numDOF:6)
        nodeCoord_new(:, 2) = nodeCoord(:, 2) + node_disp_new(2:numDOF:6)
        nodeCoord_new(:, 3) = nodeCoord(:, 3) + node_disp_new(3:numDOF:6) 

        !判断迭代结束
        label = 0
        do jj = 1,numDOF
            if (abs(disp_gap(jj)) > 1e-7) then
                label = 1
            end if
        end do

    end do

    ! 打开文件
    open(newunit = file1, file=filename1, status='replace', action='write')

    ! 将数据写入文件
    write(*,*) "位移"
    do jj = 1,numDOF
        write(*, '(I5,1X,ES12.3E2)') jj,node_disp_new(jj)
    end do

    ! 关闭文件
    close(file1)

    ! 打开文件
    open(newunit = file2, file=filename2, status='replace', action='write')

    ! 将数据写入文件
    do jj = 1,numNode
        write(file2, '(ES12.3E2)') nodeCoord_new(jj,:)
    end do

    ! 关闭文件
    close(file2)

    !释放申请的变量内存
    deallocate(stiffness,node_disp,total_M,node_speed,disp_gap,node_acc_speed,nonlinear_stiffness,force_internal,node_disp_new)

    return
end subroutine 

!求解单元刚度矩阵、单元质量矩阵
!组装整体刚度矩阵
subroutine assembly_matrix(xx,yy,zz,EleNode,force,nonlinear_stiffness,disp_gap,stiffness,total_M, &
                            node_speed,node_acc_speed,force_internal)
    !param1:全局坐标系x轴方向的节点坐标
    !param2:全局坐标系y轴方向的节点坐标
    !param3:全局坐标系z轴方向的节点坐标
    !param4:单元节点信息
    !param5:整体外载荷
    !param6:几何非线性整体刚度矩阵
    !param7:记录迭代过程中，全局坐标系下的位移增量
    !param8:整体刚度矩阵
    !param9:整体质量矩阵
    !param10:节点速度
    !param11:节点加速度
    !param12:单元内力

    !调用module中的常量
    use input_module
    use envir_param

    !对函数的形参声明
    integer :: EleNode(numEle,2)
    real :: xx(numNode)
    real :: yy(numNode)
    real :: zz(numNode)
    real :: force(numDOF)
    real :: nonlinear_stiffness(numDOF,numDOF)
    real :: disp_gap(numDOF)
    real :: stiffness(numDOF,numDOF)
    real :: total_M(numDOF,numDOF)
    real :: node_speed(numDOF)
    real :: node_acc_speed(numDOF)
    real :: force_internal(numDOF)

    !12x12的局部坐标到整体坐标的转换矩阵
    real :: T(12,12)

    !3x3的局部坐标到整体坐标的转换矩阵
    real :: Tf(3,3)

    !单元刚度矩阵
    real :: eleK(12,12)

    !12x12坐标转换矩阵的逆矩阵
    real :: T_inv(12,12)

    !12x12坐标转换矩阵转置后的逆矩阵
    real :: T_trans_inv(12,12)

    !3x3坐标转换矩阵的逆矩阵
    real :: Tf_inv(3,3)

    !3x3坐标转换矩阵转置后的逆矩阵
    real :: Tf_trans_inv(3,3)

    !几何非线性刚度矩阵
    real :: Knl(12,12)

    !局部坐标系下，水流速度分量
    real :: V_local(3)

    !全局坐标系下，单元节点1的速度
    real :: node_speed_1(3)

    !全局坐标系下，单元节点2的速度
    real :: node_speed_2(3)

    !全局坐标系下，单元节点1的加速度
    real :: node_acc_speed_1(3)
    
    !全局坐标系下，单元节点2的加速度
    real :: node_acc_speed_2(3)

    !单元质量矩阵
    real :: eleM(12,12)

    !局部坐标系下，单元速度
    real :: elem_speed(3)

    !局部坐标系下，单元加速度
    real :: elem_acc_speed(3)
    
    !全局坐标系下，单元流体力
    real :: F_globa(3)

    !单元结构阻尼矩阵
    real :: damp_matrix_ele(12,12)
    
    !局部坐标系下迭代位移增量
    real :: displacement_local(12)
    
    !单元刚度矩阵
    real :: K_element(12,12)

    !单元的真实长度
    real :: L

    !方向余弦法求坐标转换矩阵的参数
    real :: lx,mx,nx,ly,my,ny,lz,mz,nz,D

    !求解单元刚度矩阵的参数
    real :: EAL,EIL1,EIL2,EIL3,GIL
    
    !morison局部单元水动力载荷
    real :: Fy,Fz

    !计算变量
    integer :: ii,jj

    !单元的节点编号
    integer :: noindex(2)

    !记录整体矩阵中单元对应的自由度
    integer :: eleDof_t(3),eleDof(12)

    !调用一些函数接口
    interface

        !余弦法求解坐标转换矩阵12x12
        function coordinate_transform_matrix(lx,mx,nx,ly,my,ny,lz,mz,nz) result(matrix)
            implicit none
            real :: lx,mx,nx,ly,my,ny,lz,mz,nz
            real, dimension(12, 12) :: matrix
        end function coordinate_transform_matrix

        !余弦法求解坐标转换矩阵3x3
        function coordinate_transform_matrix_force(lx,mx,nx,ly,my,ny,lz,mz,nz) result(matrix)
            implicit none
            real :: lx,mx,nx,ly,my,ny,lz,mz,nz
            real, dimension(3, 3) :: matrix
        end function coordinate_transform_matrix_force

        !求解单元刚度矩阵
        function stiffness_matrix(EAL,EIL1,EIL2,EIL3,GIL) result(eleK)
            implicit none
            real :: EAL,EIL1,EIL2,EIL3,GIL
            real, dimension(12, 12) :: eleK
        end function stiffness_matrix

        !求解单元质量矩阵
        function mass_matrix(p, A, L, Ip) result(eleM)
            implicit none
            real :: p, A, L, Ip
            real, dimension(12, 12) :: eleM
        end function

        !全拉格朗日法求解切向刚度矩阵
        function tangential_stiffness_matrix(L,t_gap,disp_gap,noindex,Tf_trans_inv) result(Knl)
            use input_module
            implicit none
            real :: L,t_gap,disp_gap(numDOF),Tf_trans_inv(3,3),param1(3),param2(3),param3(3),param4(3)
            real, dimension(12, 12) :: Knl
            integer :: noindex(2)
        end function

    end interface

    do ii = 1,numEle
        noindex = EleNode(ii,:)
        deltax = xx(noindex(2))-xx(noindex(1))
        deltay = yy(noindex(2))-yy(noindex(1))
        deltaz = zz(noindex(2))-zz(noindex(1))
        L = sqrt(deltax**2 + deltay**2 + deltaz**2)

        lx = deltax/L
        mx = deltay/L
        nx = deltaz/L
        D = sqrt(lx**2+mx**2)

        !当单元轴向与全局z轴重合时，进行特殊调整
        if (D == 0) then
            lx = 0
            mx = 0
            nx = 1
            ly = 0
            my = 1
            ny = 0
            lz = -1
            mz = 0
            nz = 0
        else
            ly = -mx/D
            my = lx/D
            ny = 0
            lz = -lx*nx/D
            mz = -mx*nx/D
            nz = D
        endif

        !求解坐标转换矩阵
        T = coordinate_transform_matrix(lx,mx,nx,ly,my,ny,lz,mz,nz)
        Tf = coordinate_transform_matrix_force(lx,mx,nx,ly,my,ny,lz,mz,nz);

        !求解转换矩阵的逆矩阵
        call invert_matrix(T,T_inv,12)
        call invert_matrix(transpose(T),T_trans_inv,12)
        call invert_matrix(Tf,Tf_inv,3)
        call invert_matrix(transpose(Tf),Tf_trans_inv,3)

        !求解单元刚度矩阵
        EAL = E*A/L
        EIL1 = E*I/L
        EIL2 = 6*E*I/(L*L)
        EIL3 = 12*E*I/(L*L*L)
        GIL = G*Ip/L
        eleK = stiffness_matrix(EAL,EIL1,EIL2,EIL3,GIL)

        !求解单元几何非线性刚度矩阵
        Knl = tangential_stiffness_matrix(L,t_gap,disp_gap,noindex,Tf_trans_inv);
        
        !求解单元质量矩阵
        eleM = mass_matrix(p,A,L,Ip);

        !morison force
        !对于均匀载荷，均分到节点
        V_local = matmul(Tf_trans_inv,(/Vx,Vy,0.0/))
        node_speed_1 = [node_speed(noindex(1)*6-5),node_speed(noindex(1)*6-4),node_speed(noindex(1)*6-3)];
        node_speed_2 = [node_speed(noindex(2)*6-5),node_speed(noindex(2)*6-4),node_speed(noindex(2)*6-3)];
        node_acc_speed_1 = [node_acc_speed(noindex(1)*6-5),node_acc_speed(noindex(1)*6-4),node_acc_speed(noindex(1)*6-3)];
        node_acc_speed_2 = [node_acc_speed(noindex(2)*6-5),node_acc_speed(noindex(2)*6-4),node_acc_speed(noindex(2)*6-3)];
        elem_speed = 0.5 * matmul(Tf_trans_inv,(node_speed_1 + node_speed_2));
        elem_acc_speed = 0.5 * matmul(Tf_trans_inv,(node_acc_speed_1 + node_acc_speed_2));

        !求解单元局部法向方向的水动力载荷
        Fy=(pw*Cd*Diameter*L/2)*(V_local(2) - elem_speed(2))*((V_local(2) - elem_speed(2))**2 + (V_local(3) &
            - elem_speed(3))**2)**0.5 + pw*(1 + Ca)*pi*(Diameter**2)/4*L*V_a2 - pw*Ca*pi*(Diameter**2)/4*L*elem_acc_speed(2);
        Fz=(pw*Cd*Diameter*L/2)*(V_local(3) - elem_speed(3))*((V_local(2) - elem_speed(2))**2 + (V_local(3) &
            - elem_speed(3))**2)**0.5 + pw*(1 + Ca)*pi*(Diameter**2)/4*L*V_a3 - pw*Ca*pi*(Diameter**2)/4*L*elem_acc_speed(3);

        !将单元水动力转换到全局坐标系下的节点上
        F_globa = matmul(transpose(Tf),(/0.0,Fy,Fz/))
        eleDof_t = [noindex(1)*6-5,noindex(1)*6-4,noindex(1)*6-3]
        force(eleDof_t) = force(eleDof_t) + 0.5*F_globa
        eleDof_t = [noindex(2)*6-5,noindex(2)*6-4,noindex(2)*6-3]
        force(eleDof_t) = force(eleDof_t) + 0.5*F_globa
        
        !单元节点自由度的位置
        eleDof = [noindex(1)*6-5,noindex(1)*6-4,noindex(1)*6-3,noindex(1)*6-2,noindex(1)*6-1,noindex(1)*6, &
                  noindex(2)*6-5,noindex(2)*6-4,noindex(2)*6-3,noindex(2)*6-2,noindex(2)*6-1,noindex(2)*6]

        !求解整体刚度矩阵
        stiffness(eleDof,eleDof) = stiffness(eleDof,eleDof)+matmul(matmul(transpose(T),eleK),T)

        !求解整体几何非线性刚度矩阵
        nonlinear_stiffness(eleDof,eleDof) = nonlinear_stiffness(eleDof,eleDof)+matmul(matmul(transpose(T),Knl),T)

        !求解整体质量矩阵
        total_M(eleDof,eleDof) = total_M(eleDof,eleDof)+matmul(matmul(transpose(T),eleM),T)

        !求解单元内力
        if (dynamic == 1) then
            damp_matrix_ele = damp_param_2*eleK
            K_element = elek + c0*eleM + c1*damp_matrix_ele + Knl
        else
            K_element = elek + Knl
        end if
        displacement_local = matmul(T_trans_inv,disp_gap(eleDof))
        force_internal(eleDof) = matmul(T_trans_inv,force_internal(eleDof))
        force_internal(eleDof) = force_internal(eleDof) + matmul(K_element,displacement_local)
        force_internal(eleDof) = matmul(T_inv,force_internal(eleDof))
    
    end do

end subroutine

!建立网衣数值模型
!进行动态响应分析
subroutine dynamic_analysis()
    use envir_param
    use input_module
    use array_statement
    implicit none

    !为全局变量赋值
    call calculateNumNodeAndNumEle()

    !为变量申请内存
    allocate(force(numDOF), force_static(numDOF),restrainedDof(numrestrain),activeDof(numactive))
    allocate(nodeCoord(numNode,3),EleNode(numEle,2))

    !模型参数建立
    call modeling(force_static,nodeCoord,EleNode)       

    !设定约束条件        
    call constraint(restrainedDof,activeDof)           

    !计算重力和浮力         
    call gravity(force_static,EleNode)                       

    !静态和动态计算   
    if (dynamic == 1) then
        call time_domain_dynamic(force,nodeCoord,force_static,EleNode,activeDof)
    else
        print*,"test_static"
        call time_domain_static(force,nodeCoord,force_static,EleNode,activeDof)
    end if 

    !释放申请的变量内存
    deallocate (force,force_static,nodeCoord,EleNode,restrainedDof,activeDof)
    return 
end subroutine 

!网衣数值分析的main函数
program main
    implicit none
    call dynamic_analysis()
    read(*,*) 
end program main


!一些功能性函数
!求出两个数组的差集
subroutine setdiff(a, b, c, n, m, k)
    ! Find the difference set of two arrays 
    ! a: First array
    ! b: 第二个数组
    ! c: 差集数组
    ! n: 第一个数组的长度
    ! m: 第二个数组的长度
    ! k: 差集数组的长度

    integer n,m,k
    integer :: a(*),b(*),c(*)
    
    !计数变量
    integer :: i, j, l, count

    count = 0
    do i = 1, n
        do j = 1, m
            if (a(i) == b(j)) then
                exit
            endif
        end do
        if (a(i) == b(j)) cycle
        count = count + 1
        c(count) = a(i)
        !print*,"count(",count,")",c(count)
    end do
    !错误处理
    if (count /= k) then
        print*,"setdiff count err"
    end if

    return
end subroutine setdiff

!采用消元法求解矩阵的逆矩阵
subroutine invert_matrix(a, inverse,n)
    !param1:原矩阵
    !param2:逆矩阵
    !param3:矩阵维数

    implicit none
    integer :: n
    real, intent(in) :: a(n,n)
    real, intent(out) :: inverse(n,n)

    !临时矩阵
    real :: temp_a(n,n), temp_inverse(n,n)

    !行列式值，判读矩阵是否可逆
    real :: determinant

    !计数变量
    integer :: i, j, k
    
    !记录最大值所在行数
    integer :: max_row

    !临时变量
    real :: temp, temp_row(n)

    ! 创建单位矩阵
    inverse = 0.0
    do i = 1, n
        inverse(i,i) = 1.0
    end do

    temp_a = a
    temp_inverse = inverse
    determinant = 1.0

    do i = 1, n
        ! 检查主对角线上的元素是否为零
        if (temp_a(i,i) == 0.0) then
            ! 主对角线上存在零时，进行换行处理
            ! 查找下方的行中，该列元素绝对值最大的行
            max_row = i
            do j = 1, n
                if (abs(temp_a(j,i)) > abs(temp_a(max_row,i))) then
                    max_row = j
                end if
            end do

            ! 交换行
            if (max_row /= i) then
                temp_row = temp_a(i,:)
                temp_a(i,:) = temp_a(max_row,:)
                temp_a(max_row,:) = temp_row

                temp_row = temp_inverse(i,:)
                temp_inverse(i,:) = temp_inverse(max_row,:)
                temp_inverse(max_row,:) = temp_row

                determinant = -determinant
            else
                determinant = 0.0
                return
            end if
        end if

        ! 继续高斯-约旦消元法
        temp = temp_a(i,i)
        temp_a(i,:) = temp_a(i,:) / temp
        temp_inverse(i,:) = temp_inverse(i,:) / temp
        determinant = determinant * temp

        do j = 1, n
            if (j /= i) then
                temp = temp_a(j,i)
                temp_a(j,:) = temp_a(j,:) - temp * temp_a(i,:)
                temp_inverse(j,:) = temp_inverse(j,:) - temp * temp_inverse(i,:)
            end if
        end do
    end do

    !判读矩阵行列式是否为0
    if (determinant == 0.0) then
        write(*,*) "Matrix is not invertible."
    end if

    !输出逆矩阵
    inverse = temp_inverse

    return
end subroutine invert_matrix

!系统矩阵以及转换矩阵

!计算12x12局部坐标到全局坐标转换矩阵
function coordinate_transform_matrix(lx,mx,nx,ly,my,ny,lz,mz,nz) result(matrix)
    implicit none
    real :: lx,mx,nx,ly,my,ny,lz,mz,nz
    real, dimension(12, 12) :: matrix 

    matrix = reshape((/ &
        lx,mx,nx,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0, &
        ly,my,ny,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0, &
        lz,mz,nz,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0, &
        0.0,0.0,0.0,lx,mx,nx,0.0,0.0,0.0,0.0,0.0,0.0, &
        0.0,0.0,0.0,ly,my,ny,0.0,0.0,0.0,0.0,0.0,0.0, &
        0.0,0.0,0.0,lz,mz,nz,0.0,0.0,0.0,0.0,0.0,0.0, &
        0.0,0.0,0.0,0.0,0.0,0.0,lx,mx,nx,0.0,0.0,0.0, &
        0.0,0.0,0.0,0.0,0.0,0.0,ly,my,ny,0.0,0.0,0.0, &
        0.0,0.0,0.0,0.0,0.0,0.0,lz,mz,nz,0.0,0.0,0.0, &
        0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,lx,mx,nx, &
        0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,ly,my,ny, &
        0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,lz,mz,nz /), &
    shape(matrix))

    !转置列优先矩阵
    matrix = transpose(matrix)

    return
end function

!计算3x3局部坐标到全局坐标转换矩阵
function coordinate_transform_matrix_force(lx,mx,nx,ly,my,ny,lz,mz,nz) result(matrix)
    implicit none
    real :: lx,mx,nx,ly,my,ny,lz,mz,nz
    real, dimension(3, 3) :: matrix 
    matrix = reshape((/ &
        lx,mx,nx, &
        ly,my,ny, &
        lz,mz,nz /), &
    shape(matrix))

    !转置列优先矩阵
    matrix = transpose(matrix)

    return
end function

!计算单元刚度矩阵
function stiffness_matrix(EAL,EIL1,EIL2,EIL3,GIL) result(eleK)
    implicit none
    real :: EAL,EIL1,EIL2,EIL3,GIL
    real, dimension(12, 12) :: eleK
    
    eleK = reshape((/ &
        EAL, 0.0, 0.0, 0.0, 0.0, 0.0,-EAL, 0.0, 0.0, 0.0, 0.0, 0.0, &
        0.0, EIL3, 0.0, 0.0, 0.0, EIL2, 0.0,-EIL3, 0.0, 0.0, 0.0, EIL2, &
        0.0, 0.0, EIL3, 0.0,-EIL2, 0.0, 0.0, 0.0,-EIL3, 0.0,-EIL2, 0.0, &
        0.0, 0.0, 0.0, GIL, 0.0, 0.0, 0.0, 0.0, 0.0,-GIL, 0.0, 0.0, &
        0.0, 0.0,-EIL2, 0.0,4.0*EIL1, 0.0, 0.0, 0.0, EIL2, 0.0,2.0*EIL1, 0.0, &
        0.0, EIL2, 0.0, 0.0, 0.0,4.0*EIL1, 0.0,-EIL2, 0.0, 0.0, 0.0,2.0*EIL1, &
        -EAL, 0.0, 0.0, 0.0, 0.0, 0.0, EAL, 0.0, 0.0, 0.0, 0.0, 0.0, &
        0.0,-EIL3, 0.0, 0.0, 0.0,-EIL2, 0.0, EIL3, 0.0, 0.0, 0.0,-EIL2, &
        0.0, 0.0,-EIL3, 0.0, EIL2, 0.0, 0.0, 0.0, EIL3, 0.0, EIL2, 0.0, &
        0.0, 0.0, 0.0,-GIL, 0.0, 0.0, 0.0, 0.0, 0.0, GIL, 0.0, 0.0, &
        0.0, 0.0,-EIL2, 0.0,2.0*EIL1, 0.0, 0.0, 0.0, EIL2, 0.0,4.0*EIL1, 0.0, &
        0.0, EIL2, 0.0, 0.0, 0.0, 2*EIL1, 0.0, -EIL2, 0.0, 0.0, 0.0, 4*EIL1 /), &
    shape(eleK))

    !转置列优先矩阵
    eleK = transpose(eleK)

    return
end function

function mass_matrix(p, A, L, Ip) result(eleM)
    implicit none
    real :: p, A, L, Ip
    real, dimension(12, 12) :: eleM
    
    eleM = reshape((/ &
                140.0, 0.0, 0.0, 0.0, 0.0, 0.0, 70.0, 0.0, 0.0, 0.0, 0.0, 0.0, &
                0.0, 156.0, 0.0, 0.0, 0.0, 22.0*L, 0.0, 54.0, 0.0, 0.0, 0.0, -13.0*L, &
                0.0, 0.0, 156.0, 0.0, 22.0*L, 0.0, 0.0, 0.0, 54.0, 0.0, -13.0*L, 0.0, &
                0.0, 0.0, 0.0, 140.0*Ip/A, 0.0, 0.0, 0.0, 0.0, 0.0, 70.0*Ip/A, 0.0, 0.0, &
                0.0, 0.0, 22.0*L, 0.0, 4.0*L**2, 0.0, 0.0, 0.0, 13.0*L, 0.0, -3.0*L**2, 0.0, &
                0.0, 22.0*L, 0.0, 0.0, 0.0, 4.0*L**2, 0.0, 13.0*L, 0.0, 0.0, 0.0, -3.0*L**2, &
                70.0, 0.0, 0.0, 0.0, 0.0, 0.0, 140.0, 0.0, 0.0, 0.0, 0.0, 0.0, &
                0.0, 54.0, 0.0, 0.0, 0.0, 13.0*L, 0.0, 156.0, 0.0, 0.0, 0.0, -22.0*L, &
                0.0, 0.0, 54.0, 0.0, 13.0*L, 0.0, 0.0, 0.0, 156.0, 0.0, -22.0*L, 0.0, &
                0.0, 0.0, 0.0, 70.0*Ip/A, 0.0, 0.0, 0.0, 0.0, 0.0, 140.0*Ip/A, 0.0, 0.0, &
                0.0, 0.0, -13.0*L, 0.0, -3.0*L**2, 0.0, 0.0, 0.0, -22.0*L, 0.0, 4.0*L**2, 0.0, &
                0.0, -13.0*L, 0.0, 0.0, 0.0, -3.0*L**2, 0.0, -22.0*L, 0.0, 0.0, 0.0, 4.0*L**2 /), &
    shape(eleM))

    !转置列优先矩阵
    eleM = (p*A*L/420.0) * transpose(eleM)

    return
end function mass_matrix

!几何非线性刚度矩阵
function tangential_stiffness_matrix(L,t_gap,disp_gap,noindex,Tf_trans_inv) result(Knl)
    use input_module
    implicit none

    !单元长度
    real :: L

    !时间步长度
    real :: t_gap

    !增量位移
    real :: disp_gap(numDOF)

    !3x3坐标转换矩阵在转置后的逆矩阵
    real :: Tf_trans_inv(3,3)

    !自定义变量，简化计算
    real :: param1(3)
    real :: param2(3)
    real :: param3(3)
    real :: param4(3)
    
    !全局坐标系下，单元节点的位移和转角
    real :: u1,v1,w1,cx1,cy1,cz1,u2,v2,w2,cx2,cy2,cz2

    !单元节点信息
    integer :: noindex(2)

    !几何非线性单元刚度矩阵
    real, dimension(12, 12) :: Knl

    !调用接口函数
    interface
        !求解几何非线性单元刚度矩阵
        function tangential_stiffness_matrix_sci(L,t,u1,v1,w1,cx1,cy1,cz1,u2,v2,w2,cx2,cy2,cz2) result(matrix)
            use input_module
            implicit none
            real :: L,t,u1,v1,w1,cx1,cy1,cz1,u2,v2,w2,cx2,cy2,cz2
            real, dimension(12, 12) :: matrix
        end function
    end interface

    u1 = disp_gap(noindex(1)*6-5)
    v1 = disp_gap(noindex(1)*6-4)
    w1 = disp_gap(noindex(1)*6-3)
    u2 = disp_gap(noindex(2)*6-5)
    v2 = disp_gap(noindex(2)*6-4)
    w2 = disp_gap(noindex(2)*6-3)
    cx1 = disp_gap(noindex(1)*6-2)
    cy1 = disp_gap(noindex(1)*6-1)
    cz1 = disp_gap(noindex(1)*6)
    cx2 = disp_gap(noindex(2)*6-2)
    cy2 = disp_gap(noindex(2)*6-1)
    cz2 = disp_gap(noindex(2)*6)
    param1 = matmul(Tf_trans_inv,(/u1,v1,w1/))
    param2 = matmul(Tf_trans_inv,(/u2,v2,w2/))
    param3 = matmul(Tf_trans_inv,(/cx1,cy1,cz1/))
    param4 = matmul(Tf_trans_inv,(/cx2,cy2,cz2/))

    Knl = tangential_stiffness_matrix_sci(L,t_gap,param1(1),param1(2),param1(3), &
        param3(1),param3(2),param3(3),param2(1),param2(2),param2(3),param4(1),param4(2),param4(3));

end function tangential_stiffness_matrix

!求解几何非线性单元刚度矩阵
function tangential_stiffness_matrix_sci(L,t,u1,v1,w1,cx1,cy1,cz1,u2,v2,w2,cx2,cy2,cz2) result(matrix)
    use input_module
    implicit none

    !单元长度
    real :: L

    !时间步长
    real :: t

    !局部坐标系下，单元节点位移及转角
    real :: u1,v1,w1,cx1,cy1,cz1,u2,v2,w2,cx2,cy2,cz2

    !临时变量，用于简化计算
    real :: D,u,v,w,cx,cy,cz,c1,c2,c3,c4,c5

    !输出矩阵
    real, dimension(12, 12) :: matrix

    !初应力矩阵
    real, dimension(12, 12) :: K_stress_1
    real, dimension(12, 12) :: K_stress_2
    real, dimension(12, 12) :: K_stress_3

    !大位移矩阵
    real, dimension(12, 12) :: K_large_1
    real, dimension(12, 12) :: K_large_2
    real, dimension(12, 12) :: K_large_3
    real, dimension(12, 12) :: K_large_4
    
    !网目直径
    D = Diameter

    !初始化矩阵
    K_stress_1(:,:) = 0
    K_stress_2(:,:) = 0
    K_stress_3(:,:) = 0
    K_large_1(:,:) = 0
    K_large_2(:,:) = 0
    K_large_3(:,:) = 0
    K_large_4(:,:) = 0

    u = u1 - u2;
    v = v1 - v2;
    w = w1 - w2;
    cx = cx1 - cx2;
    cy = cy1 - cy2;
    cz = cz1 - cz2;
    c1 = cy1+cy2;
    c2 = cz1+cz2;
    c3 = cy1*cy2 +cz1*cz2;
    c4 = cy1**2 + cy2**2;
    c5 = cz1**2 + cz2**2;

    !直接通过显示表达式计算各个矩阵
    !初应力矩阵分量1
    K_stress_1(7,7) = ( (-2.0*L*u+u**2)/(2.0*L**3) + 3.0*(v**2+w**2)/(5.0*L**3)+ (2.0*cz**2+2.0*cy**2+3.0*c3)/(30.0*L)) &
        + E*A*(v*c2 - w*c1)/(10.0/L**2) + E*I/L**3;

    K_stress_1(1,1) = K_stress_1(7,7) 

    K_stress_1(1,7) = -K_stress_1(1,1)

    !初应力矩阵分量2
    K_stress_2(2,2) = 36.0*E*A*(w**2+v**2)/(35.0*L**3) - 9.0*E*A*(w*c1-v*c2)/(35.0*L**2) + 3.0*E*A*(c4+c5)/(70.0*L) &
        + 3.0*E*2.0*I*cx**2/(5.0*L)

    K_stress_2(4,2) = E*I*cx*(12.0*v+L*c2)/(10.0*L**3)

    K_stress_2(4,4) = E*I*(3.0*(v**2+w**2)/(5.0*L**3) + (v*c2-w*c1)/(10.0*L**2) - c3/(30.0*L) + (c4+c5)/(15.0*L)) &
        - (2.0*u+u**2)*E*I/L**3

    K_stress_2(6,2) = (-u/(10.0*L)+u**2/(20.0*L**2) - 3.0*(cy1*w-cz1*v)/(35.0*L) - (cz*c2+cy*c1)/280.0 + c3/140.0)*E*A &
        + 9.0*(v**2+w**2)*E*A/(70.0*L**2) + 2.0*E*I*cx/(20.0*L**2)

    K_stress_2(6,4) = E*I*cx*(-3.0*v+L*(cz2-4.0*cz1))/(30.0*L**2)

    K_stress_2(6,6) = (3.0*(v**2+w**2)/(70.0*L) - (v*cz-w*cy)/140.0 - L*c3/140.0 + L*(cz1**2+cy1**2)/35.0 &
        + L*(cz2**2+cy2**2)/420.0)*E*A + 2.0*I*E*cx**2/(15.0*L) + u**2*E*A/(15.0*L) - 2.0*u*E*A/15.0

    K_stress_2(12,2) = (-u/(10.0*L)+u**2/(20.0*L**2) - 3.0*(cy2*w-cz2*v)/(35.0*L) + (cz*c2+cy*c1)/280.0 + c3/140.0)*E*A &
        + 9.0*(v**2+w**2)*E*A/(70.0*L**2) + 2.0*E*I*cx/(20.0*L**2)

    K_stress_2(12,4) = E*I*cx*(-3.0*v+L*(cz1-4.0*cz2))/(30.0*L**2)

    K_stress_2(12,6 ) = (-(w*c1-v*c2)/140.0 - L*c3/210.0 - L*(c4+c5)/280.0)*E*A - 2.0*I*E*cx**2/(60.0*L) &
        - u**2*E*A/(30.0*L) + u*E*A/30.0

    K_stress_2(12,12) = (3.0*(v**2*w**2)/(70.0*L) + (v*cz-w*cy)/140.0 - L*c3/140.0 + L*(cz2**2+cy2**2)/35.0 &
        + L*(cz1**2+cy1**2)/420.0)*E*A + 2.0*I*E*cx**2/(15.0*L) + u**2*E*A/(15.0*L) - 2.0*u*E*A/15.0

    !初应力矩阵分量3
    K_stress_3(3,3) = 36.0*E*A*(w**2+v**2)/(35.0*L**2)-9.0*E*A*(w*c1-v*c2)/(35.0*L**2) &
        +3.0*E*A*(cy1**2+cy2**2+cz1**2+cz2**2)/(70.0*L)+3.0*E*2*I*cx**2/(5.0*L)

    K_stress_3(4,3) = E*I*cx*(12.0*w-L*c1)/(10.0*L**3)

    K_stress_3(4,4) = E*I*(3.0*(v**2+w**2)/(5.0*L**3)+(v*c2-w*c1)/(10.0*L**2)-c3/(30.0*L)+(c4+c5)/(15.0*L)) &
        -(2.0*u+u**2)*E*I/L**3

    K_stress_3(5,4) = E*I*cx*(-3.0*w+L*(4*cy1-cy2))/(30.0*L**2)

    K_stress_3(5,3) = (u/(10.0*L)-u**2/(20.0*L**2)+3.0*(cy1*w-cz1*v)/(35.0*L)+(cz*c2+cy*c1)/280.0-c3/140.0)*E*A &
        -9.0*(v**2+w**2)*E*A/(70.0*L**2)-E*2*I*cx/(20.0*L**2)
        
    K_stress_3(5,5) = (3.0*(v**2+w**2)/(70.0*L)-(v*cz-w*cy)/140.0-L*c3/140.0+L*(cz1**2+cy1**2)/35.0 &
        +L*(cz2**2+cy2**2)/420.0)*E*A+2*I*E*cx**2/(15.0*L)+u**2*E*A/(15.0*L)-2*u*E*A/15.0

    K_stress_3(11,3) = (u/(10.0*L)-u**2/(20.0*L**2)+3.0*(cy2*w-cz2*v)/(35.0*L)+(cz*c2+cy*c1)/280.0-c3/140.0)*E*A &
        -9.0*(v+w)*w*E*A/(70.0*L**2)-E*2*I*cx/(20.0*L**2)

    K_stress_3(11,4) = E*I*cx*(-3.0*w-L*(cy1-4*cy2))/(30.0*L**2)

    K_stress_3(11,5) = (-(w*c1-v*c2)/140.0-L*c3/210.0+L*(c4+c5)/280.0)*E*A-2*I*E*cx**2/(60.0*L)-u**2*E*A/(30.0*L)+u*E*A/15.0

    K_stress_3(11,11) = (3.0*(v**2+w**2)/(70.0*L)-(v*cz-w*cy)/140.0-L*c3/140.0+L*(cz1**2+cy1**2)/420.0 &
        +L*(cz2**2+cy2**2)/35.0)*E*A+2*I*E*cx**2/(15.0*L)+u**2*E*A/(15.0*L)-2*u*E*A/15.0

    !大位移矩阵1
    K_large_1(2,1) = -6.0*E*A*v/(5.0*L**2) - E*A*c2/(10.0*L)

    K_large_1(2,2) = (72.0*v**2/(35.0*L**3) + 18.0*v*c2/(35.0*L**2) + 3.0*c5/(35.0*L))*E*A + 6.0*E*I*cx**2/(5.0*L)

    K_large_1(4,1) = -E*I*cx/L**2

    K_large_1(4,2) = E*I*cx*(12.0*v + L*c2)/(5.0*L**3)

    K_large_1(4,4) = E*I*(6.0*v**2/(5.0*L**3) + v*c2/(5.0*L**2) - cz1*cz2/15.0 + 2.0*c5)/(15.0*L)

    K_large_1(6,1) = E*A*(-v/(10.0*L) - 2.0*cz1/15.0 + cz2/30.0)

    K_large_1(6,2) = (6.0*cz1*v/(35.0*L) - cz*c2/140.0 + cz1*cz2/70.0 + 9.0*v**2/(35.0*L**2))*E*A + E*I*cx/(10.0*L**2)

    K_large_1(6,6) = (3.0*v**2/(35.0*L) - v*cz/70.0 - L*cz1*cz2/70.0 + 2.0*L*cz1**2/35.0 + L*cz2**2/210.0)*E*A &
        + 2.0*E*I*cx**2/(15.0*L)

    K_large_1(12,1) = E*A*(-v/(10.0*L) - 2.0*cz2/15.0 + cz1/30.0)

    K_large_1(12,2) = (6.0*cz2*v/(35.0*L) + cz*c2/140.0 + cz1*cz2/70.0 + 9.0*v**2/(35.0*L**2))*E*A + E*I*cx/(10.0*L**2)

    K_large_1(12,6) = (v*c2/70.0 - L*c5/140.0 + L*cz2**2/105.0)*E*A - E*I*cx**2/(30.0*L)

    K_large_1(12,12) = (3.0*v**2/(35.0*L) + v*cz/70.0 + L*cz1**2/210.0 - L*cz1*cz2/70.0 + 2.0*L*cz2**2/35.0)*E*A &
        + 2.0*E*I*cx**2/(15.0*L)

    K_large_1(8,1) = -K_large_1(2,1)

    K_large_1(7,2) = K_large_1(8,1)

    K_large_1(7,4) = -K_large_1(4,1)

    K_large_1(10,1) = K_large_1(7,4)

    K_large_1(10,4) = -K_large_1(4,4)

    K_large_1(8,7) = K_large_1(2,1)

    K_large_1(8,8) = K_large_1(2,2)

    K_large_1(10,8) = K_large_1(4,2)

    K_large_1(10,10) = K_large_1(4,4)

    !大位移矩阵2
    K_large_2(3,1)=-6.0*E*A*w/(5.0*L**2)+E*A*c1/(10.0*L)

    K_large_2(3,3)=(72.0*w**2/(35.0*L**3)-18.0*w*c1/(35.0*L**2)+3.0*c4/(35.0*L))*E*A+6.0*E*I*cx**2/(5.0*L)

    K_large_2(4,1)=-E*I*cx/L**2

    K_large_2(4,3)=E*I*cx*(12.0*w+L*c1)/(5.0*L**3)

    K_large_2(4,4)=E*I*(6.0*w**2/(5.0*L**3)-w*c1/(5.0*L**2)-cy1*cy2/15.0+2.0*c4/(15.0*L))

    K_large_2(5,1)=E*A*(w/(10.0*L)-2.0*cy1/15.0+cy2/30.0)

    K_large_2(5,3)=(6.0*cy1*w/(35.0*L)+cy*c1/140.0-cy1*cy2/70.0-9.0*w**2/(35.0*L**2))*E*A-E*I*cx/(10.0*L**2)

    K_large_2(5,5)=(3.0*w**2/(35.0*L)+w*cy/70.0-L*cy1*cy2/70.0+2.0*L*cy1**2/35.0+L*cy2**2/210.0)*E*A+2.0*E*I*cx**2/(15.0*L)

    K_large_2(11,1)=E*A*(w/(10.0*L)-2.0*cy2/15.0+cy1/30.0)

    K_large_2(11,3)=(6.0*cy2*w/(35.0*L)-cz*c2/140.0+cy1*cy2/70.0-9.0*w**2/(35.0*L**2))*E*A-E*I*cx**2/(10.0*L**2)

    K_large_2(11,5)=(-w*c2/70.0-L*c4/140.0+L*cy2*cy2/105.0)*E*A-E*I*cx**2/(30.0*L)

    K_large_2(11,11)=(3.0*w**2/(35.0*L)-w*cz/70.0+L*cy1**2/210.0-L*cy1*cy2/70.0+2.0*L*cy2**2/35.0)*E*A+2.0*E*I*cx**2/(15.0*L)

    !大位移矩阵3
    K_large_3(3,2)=(72.0*w*v/(35.0*L**3)-9.0*v*c1/(35.0*L**2)+9.0*c2*w/(35.0*L**2)+3.0*c3/(35.0*L))*E*A

    K_large_3(4,2)=E*I*cx*(12.0*v+L*c2)/(5.0*L**3)

    K_large_3(4,3)=E*I*cx*(12.0*w-L*c1)/(5.0*L**3)

    K_large_3(5,2)=(-3.0*w*cz1/(35.0*L)+3.0*v*cy1/(35.0*L)-cz1*cy1/140.0+cz1*c1/140.0-9.0*v*w/(35.0*L**2))*E*A

    K_large_3(5,4)=E*I*cz*(-3.0*w+L*(4.0*cy1-cy2))/(15.0*L**2)

    K_large_3(6,4)=E*I*cz*(-3.0*v+L*(4.0*cz1-cz2))/(15.0*L**2)

    K_large_3(6,5)=(-3.0*w*v/(35.0*L)-v*cy1/140.0+cz*w/140.0-L*(cz1*cy2+cz2*cy1)/140.0+2.0*L*cz1*cy1/35.0+L*cz2*cy2/210.0)*E*A

    K_large_3(11,2)=(-3.0*w*cz2/(35.0*L)+3.0*v*cy2/(35.0*L)-cz2*cy1/140.0+cz2*c1/140.0-9.0*v*w/(35.0*L**2))*E*A

    K_large_3(11,4)=-E*I*cx*(3.0*w+L*(cy1-4.0*cy2))/(30.0*L**2)

    K_large_3(12,4)=E*I*cx*(3.0*v-L*(cz1-4.0*cz2))/(15.0*L**2)

    K_large_3(12,5)=(v*c1/140.0-w*c2/140.0+L*(cz1*cy2+cz2*cy1)/210.0+L*(cz1*cy1+cz2*cy2)/140.0)*E*A

    K_large_3(12,11)=(-w*cz/140.0+v*cy/140.0-3.0*v*w/(35.0*L)-L*(cz1*cy2+cz2*cy1)/140.0+2.0*L*cy2*cz2/35.0+L*cz1*cy1/210.0)*E*A

    !大位移矩阵4
    K_large_4(1,1)=E*A*(-2.0*L*u+u**2)/(L**3)

    K_large_4(2,1)=E*A*u*(12.0*v+L*c2)/(10.0*L**3)

    K_large_4(3,1)=E*A*(12.0*v*w-u*L*c1)/(10.0*L**3)

    K_large_4(4,1)=2.0*I*u*cx/L**3

    K_large_4(5,1)=E*A*u*(-3.0*w-L*cy2+4.0*L*cy1)/(30.0*L**2)

    K_large_4(6,1)=E*A*u*(3.0*v-L*cz2+4.0*L*cz1)/(30.0*L**2)

    K_large_4(11,1)=E*A*u*(-3.0*w-L*cy1+4.0*L*cy2)/(30.0*L**2)

    K_large_4(12,1)=E*A*u*(3.0*v-L*cz1+4.0*L*cz2)/(30.0*L**2)

    !组装矩阵
    matrix = K_stress_1 + K_stress_2 + K_stress_3 + K_large_1 + K_large_2 + K_large_3 + K_large_4

end function tangential_stiffness_matrix_sci